/*

====================================================Platform========================================================

- OS-Ubuntu 9.10 (Sorry!! Too Lazy to Update)
- Compiler-gcc , Editor-gedit
- To run - open terminal -> gcc filename.c -lglut -lpthreads -> ./a.out
- Make sure you have OpenGL libraries (glut) and pthread.h (you'll have this)

====================================================Description===================================================== 

-Each Position on the map is an instance of struct gridPoint
-Each Position has a x cordinate, y coordinate, a state and a pointer to the time line associated with the position
-initialMap() initializes all positions to be zero
-printMap() prints the map along with the time line
-printTimeline() prints the time line
-printStates() prints the states (1,0,-1)
-addTime() adds a time element according to the value
-waitTime() calculates the time required to  wait at a gridPoint if the bot arrives at a particular time
-clearHistory() removes the time elements from all the gridpoints so after according to the present time/specified time 
-A small test program is written in main(), you can call different functions to check them, Please read the main first

NEW ADDITION - 

A simple OpenGL Visualization :) 
 
*/

#include<stdio.h>
#include<stdlib.h>
#include<GL/glut.h>
#include<unistd.h>
#include<pthread.h>

#define area 499
#define numBots 2
#define E 1
#define xOffset 1
#define yOffset 1
#define D 4
#define IDLE -1
#define EMPTY 0
#define ANIMATION_SPEED 10

struct bot
{
	int status;
	struct gridPoint *pos;
};

struct gridPoint
{
	int state;
	struct timeElement *start;
	struct bot *onIt;
};

struct timeElement
{
	int st;
	int et;
	struct timeElement *next;
	struct timeElement *prev;
};

/*-----------------Map--------------------*/

void initialMap();
void printMap();
void clearHistory(int);
void clearH();
void addTime(int,int,int,int);
int waitTime(int,int,int,int);
void printTimeline();
void printState();
void path(int,int,int,int,int,int);


struct gridPoint *g[area][area],*ghead;
int i,j,z;

/*------------------------------------*/

/*-----------Visualization-------------*/

void init();
void display();

/*------------------------------------*/

/*----------------Bots----------------*/

void createBot(int, int, int);
void moveBot(int,int,int);
struct bot* nearestBot(int,int);
struct bot *b[numBots],*bhead;

/*------------------------------------*/

void main(int argc, char **argv)
{
int a;
//making initial map
initialMap();
//adding few elements on the timeline
/*printf("Let us assume no obstacles and two robots randomly placed at (3,2) and (1,7), the map will look like this : \n");
setState(3,2,1);
setState(1,7,1);
printMap();
scanf("%d",&a);
printf("Let us assume int the first request the bot was supposed to move something from (2,0) to (8,0) and our path planner calculated the shortest path for the nearest bot to be (3,2)->(2,2)->(2->0)->(8,0). The request came at t=0, so a time based map of the whole path will look like this (Assuming time taken to cover gridpoints is 4  units with 1 unit error) : \n");
(3,2,0);
setState(8,0,1);
addTime(2,2,3,5);
addTime(2,1,7,9);
for(i=2;i<9;i++)
	addTime(i,0,(4*(i+1))-1,(4*(i+1))+1);
printMap();
scanf("%d",&a);
printf("The path was communicated tot he bot. Let us assume another request came, the bot was supposed to move something from (0,7) to (3,0) and our path planner calculated the shortest path for the nearest bot to be (1,7)->(0,7)->(0->0)->(3,0). The request came at t=5, so a time based map of the whole path will look like this (Assuming time taken to cover gridpoints is 4  units with 1 unit error) : \n");
addTime(0,7,8,10);
for(i=0;i<7;i++)
addTime(0,6-i,(i+3)*4,((i+3)*4)+2);
for(i=1;i<4;i++)
addTime(i,0,4*(i+9),2+(4*(i+9)));
setState(1,7,0);
setState(3,0,1);
//printing the map along with timelines
printMap();
scanf("%d",&a);
//calculating wait time 
printf("In the path planner we need to calculate all possible paths, then we need to check the time lines at each instance, but we also have to calculate the wait time for a bot if it reaches a grid point at a time and it is preoccupied. Suppose the wait time for any bot trying to move in cell (2,0) at time 10 will be \n");
printf("%d",waitTime(2,0,9,11));
scanf("%d",&a);
//clearing useless time element after t=25
printf("\nSince we will not use the timelines that are of the past, so assuming at t=25 if we are getting a request, we can just delete all times less that that since they are useless in future calculations and we will save memory. So time line after deleting at t<25");
clearHistory(25);
//printing thr cleared map

//path(8,1,1,8,0,1);
//printMap();
//path(1,4,8,8,0,1);
//path(3,3,9,0,3,2);
//path(2,1,6,7,5,3);
//path(,1,8,8,0,3);
path(5,0,8,8,3,4);
printMap();
path(1,1,8,8,9,5);
path(7,2,8,8,11,6);
path(0,0,8,8,6,7);
path(4,4,8,8,5,8);
path(7,0,8,8,43,9);
for(i=0;i<numBots;i++)
	createBot(i+2,i+2,i);
printMap();

moveBot(1,1,9);
printMap();
if(nearestBot(0,2)!=NULL)
printf("%d",(nearestBot(0,2))->status);
printMap();*/
path(1,4,6,8,10,1);
path(8,5,3,3,6,6);
path(7,7,7,0,2,7);
//path(1,4,6,8,11,5);
//path(1,5,6,8,1,5);
//printMap();
//path(1,1,4,8,1,2);
//path(1,1,4,8,1,3);
path(1,1,4,8,1,4);
for(i=0;i<250;i++)
path((rand()%(area-1)),(rand()%(area-1)),(rand()%(area-1)),(rand()%(area-1)),(rand()%100),i+7);
printMap();
//clearHistory(30);
//printMap();
/*
  path(5,5,40,40,5,1);
path(5,5,40,40,5,1);
path(5,5,40,40,5,1);
path(5,5,40,40,5,1);
path(5,5,40,40,5,1);
path(5,5,40,40,5,1);
path(5,5,40,40,5,1);
path(5,5,40,40,5,1);
path(5,5,40,40,5,1);
path(5,5,40,40,5,1);
path(5,5,40,40,5,1);
path(5,5,40,40,5,1);
  path(5,5,40,40,5,1);
path(5,5,40,40,5,1);
path(5,5,40,40,5,1);
path(5,5,40,40,5,1);
path(5,5,40,40,5,1);
path(5,5,40,40,5,1);
path(5,5,40,40,5,1);
path(5,5,40,40,5,1);
path(5,5,40,40,5,1);
path(5,5,40,40,5,1);
path(5,5,40,40,5,1);
path(5,5,40,40,5,1);
path(48,10,1,20,120,1);*/
glutInit(&argc, argv);
  glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE);
  glutInitWindowSize(640, 480);
  glutCreateWindow("Simulation");
  init();
  glutDisplayFunc(display);
  glutMainLoop();
clearH();
}

/*struct bot* nearestBot(int x,int y)
{
int hop=1;
int f=0;
struct bot *near;
near=NULL;
while(near==NULL)
{
for(i=x-hop;i<=x+hop;i++)
{
	for(j=y-hop;j<=y+hop;j++)
	{
		printf("%d%d",i,j);
		if(i > 0 && j > 0 && i < area && j < area)
		{	
			printf("Hello");
			if(g[i][j]->onIt != NULL)
			{
				near==g[i][j]->onIt;
				f=1;
				break;
			}
		}		
	}
	if(f)
		break;
}
hop++;
if(hop==area)
	break;
}
}*/

void createBot(int x, int y,int id)
{

	b[id]=(struct bot*)malloc(sizeof(struct bot));
	b[id]->status=IDLE;
	b[id]->pos=g[x][y];
	g[x][y]->onIt=b[id];	
	g[x][y]->state=b[id]->status;
	bhead=b[0];
}	

void moveBot(int id, int x, int y)
{
	b[id]->pos->state=EMPTY;
	b[id]->pos->onIt=NULL;
	b[id]->pos=g[x][y];
	g[x][y]->onIt=b[id];
	g[x][y]->state=b[id]->status;
}

void initialMap()
{
for(i=0;i<area;i++)
{
for(j=0;j<area;j++)
{
g[i][j] = (struct gridPoint*)malloc(sizeof(struct gridPoint));
g[i][j]->state=EMPTY;
g[i][j]->start=NULL;
g[i][j]->onIt=NULL;
}
}
ghead=g[0][0];
}

void printMap()
{
printState();
printTimeline();
//printf("\n Enter any char to cont..\n");
}

void printTimeline()
{
struct timeElement *temp;
FILE *t;
t = fopen("timeLine.txt","w");
for(i=0;i<area;i++)
{
for(j=0;j<area;j++)
{

	fprintf(t,"Time Line of (%d,%d) : " ,i,j);
	temp=g[i][j]->start;	
	while(temp!=NULL)
	{
	fprintf(t," %d:%d ",(*temp).st,(*temp).et);
	temp=temp->next;
	}
	fprintf(t,"\n");
}
}
fclose(t);
free(temp);
}
void printState()
{
FILE *s;
s = fopen("states.txt","w");
for(i=0;i<area;i++)
{
for(j=0;j<area;j++)
	fprintf(s,"%d ",(*g[i][j]).state);
fprintf(s,"\n\n");
}
fclose(s);
}

void addTime(int x, int y,int stXY, int etXY)
{
int flag=0;
struct timeElement *temp,*t;
t = (struct timeElement*)malloc(sizeof(struct timeElement));
t->st=stXY;
t->et=etXY;
if(g[x][y]->start==NULL)
{
	g[x][y]->start=t;
	t->next=NULL;
	t->prev=NULL;
}
else
{
temp=g[x][y]->start;
if(temp->st>stXY)
{
t->next=temp;
t->prev=NULL;
g[x][y]->start=t;
temp->prev=t;
temp->next=NULL;
}
else
{
while(temp->next != NULL)
{
	if(stXY<temp->next->st)
	{	
		flag=1;
		break;
	}
	temp=temp->next;
}
t->next=temp->next;
t->prev=temp;
if(flag)
	temp->next->prev=t;
temp->next=t;
}
}
}

int waitTime(int x, int  y, int stT, int etT)
{
struct timeElement *temp;
int wait=0,sumWait=0;
temp=g[x][y]->start;
while(temp!=NULL)
{
	if((stT<=temp->et&&stT>=temp->st)||(etT<=temp->et&&etT>=temp->st))
	{
		wait=(temp->et-stT)+1;
		sumWait=sumWait + wait; 
		stT=stT+wait;
		etT=etT+wait;
	}	
/*	if(etT<=temp->et&&etT>=temp->st)
	{
		wait=(temp->et-stT)+1;
		sumWait=sumWait + wait; 
		stT=stT+wait;
		etT=etT+wait;
	}*/
	temp=temp->next;
}
	return sumWait;
}

void clearHistory(int present)
{
struct timeElement *temp;

for(i=0;i<area;i++)
{
for(j=0;j<area;j++)
{
	if(g[i][j]->start!=NULL)
	{
	while(g[i][j]->start->et<=present)
	{
		temp=g[i][j]->start;
		g[i][j]->start=g[i][j]->start->next;
		if(g[i][j]->start == NULL)
			break;
		g[i][j]->start->prev=NULL;
		free(temp);
		
	}
	}
}
}
}

void clearH()
{
struct timeElement *temp;
for(i=0;i<area;i++)
{
for(j=0;j<area;j++)
{	
	if(g[i][j]->start!=NULL)
	{
	while(g[i][j]->start!=NULL)
	{
		temp=g[i][j]->start;
		g[i][j]->start=g[i][j]->start->next;
		if(g[i][j]->start == NULL)
			break;
		g[i][j]->start->prev=NULL;
		free(temp);
		
	}
	}
	free(g[i][j]);
}
}
}

void path(int xi, int yi, int xf, int yf, int st, int id) 
{
int tx,ty;
//addTime(xi,yi,st,st+4);
while((xi < xf)&&(yi < yf))
{
//printf("\n %d , %d",xi,yi); 
st=st+D;
tx=waitTime(xi+1,yi,st-1,st+1)-1;
ty=waitTime(xi,yi+1,st-1,st+1)-1;
if(tx != ty)
{
if((tx < ty)||(xi < xf))
{
	addTime(xi,yi,st-(D-1),st+tx);
	xi=xi+1;
	st=st+tx;
}
else 
{
	addTime(xi,yi,st-(D-1),st+ty);
	yi=yi+1;
	st=st+ty;
}
}
else
{
if(xi < xf)
{
	addTime(xi,yi,st-(D-1),st+tx);
	xi=xi+1;
	st=st+tx;
}
else
{
	addTime(xi,yi,st-(D-1),st+ty);
	yi=yi+1;
	st=st+ty;
}
}
g[xi][yi]->state=id;
}
//printf("\n %d , %d\n",xf,yf);
//addTime(xf,yf,st-1,st+1); 
while((xi < xf)||(yi > yf))
{
//printf("\n %d , %d",xi,yi); 
st=st+D;
tx=waitTime(xi+1,yi,st-1,st+1)-1;
ty=waitTime(xi,yi-1,st-1,st+1)-1;
if(tx != ty)
{
if((tx < ty)&&(xi < xf))
{
	addTime(xi,yi,st-(D-1),st+tx);
	xi=xi+1;
	st=st+tx;
}
else 
{
	addTime(xi,yi,st-(D-1),st+ty);
	yi=yi-1;
	st=st+ty;
}
}
else
{
if(xi < xf)
{
	addTime(xi,yi,st-(D-1),st+tx);
	xi=xi+1;
	st=st+tx;
}
else
{
	addTime(xi,yi,st-(D-1),st+ty);
	yi=yi-1;
	st=st+ty;
}
}
g[xi][yi]->state=id;
}
//printf("\n %d , %d\n",xf,yf);
//addTime(xf,yf,st-1,st+1); 
while((xi > xf)||(yi < yf))
{
//printf("\n %d , %d",xi,yi); 
st=st+D;
tx=waitTime(xi-1,yi,st-(D-1),st+1)-1;
ty=waitTime(xi,yi+1,st-(D-1),st+1)-1;
if(tx != ty)
{
if((tx < ty)&&(xi > xf))
{
	addTime(xi,yi,st-(D-1),st+tx);
	xi=xi-1;
	st=st+tx;
}
else 
{
	addTime(xi,yi,st-(D-1),st+ty);
	yi=yi+1;
	st=st+ty;
}
}
else
{
if(xi > xf)
{
	addTime(xi,yi,st-(D-1),st+tx);
	xi=xi-1;
	st=st+tx;
}
else
{
	addTime(xi,yi,st-(D-1),st+ty);
	yi=yi+1;
	st=st+ty;
}
}
g[xi][yi]->state = id;
}
//printf("\n %d , %d\n",xf,yf);
//addTime(xf,yf,st-1,st+1); 
}
void init()
{
  glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
  gluOrtho2D(0, (area+(2*xOffset)), 0, (area+(2*yOffset)));
}

void display()
{
  	int tim;
  	/*glColor3f(1.0f,0.0f,0.0f);	// set color to a blue shade.
  	for(j=yOffset; j<area+yOffset; j++) 
 	{
		for(i=xOffset; i<area+yOffset; i++)
		{
			glColor3f(1.0f,1.0f,1.0f);
    			glBegin(GL_QUADS);		// start drawing a polygon (4 sided)
    			glVertex2i(i,j);		// Top Left
    			glVertex2i(i+1,j);		// Top Right
    			glVertex2i(i+1,j+1);	// Bottom Right
    			glVertex2i(i,j+1);		// Bottom Left	
    			glEnd();
   		}
	}
	glutSwapBuffers();*/
	
for(tim=1;tim<2000;tim++)
{
glClear(GL_COLOR_BUFFER_BIT);
	for(j=yOffset; j<area+yOffset; j++) 
	{
		for(i=xOffset; i<area+xOffset; i++)
		{
			if(tim%5==0)
			clearHistory(tim-1);
			if(g[i-xOffset][j-yOffset]->start!=NULL)
			{
			if(waitTime(i-xOffset,j-yOffset,tim,tim)!=0)
			{	glColor3f(1.0f,0.0f,0.0f);
			//else
			//	glColor3f(1.0f,1.0f,1.0f);
		
	    		glBegin(GL_QUADS);		// start drawing a polygon (4 sided)
	    		glVertex2i(i,j);		// Top Left
	    		glVertex2i(i+1,j);		// Top Right
	    		glVertex2i(i+1,j+1);	// Bottom Right
	    		glVertex2i(i,j+1);		// Bottom Left	
	    		glEnd();
			
			}}			
		}			
		
	}
glutSwapBuffers();
			usleep(ANIMATION_SPEED);	
}
}